//      ublas_helper.h
//      
//      Copyright 2012 tqlong <tqlong@espada>
//      
//      This program is free software; you can redistribute it and/or modify
//      it under the terms of the GNU General Public License as published by
//      the Free Software Foundation; either version 2 of the License, or
//      (at your option) any later version.
//      
//      This program is distributed in the hope that it will be useful,
//      but WITHOUT ANY WARRANTY; without even the implied warranty of
//      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//      GNU General Public License for more details.
//      
//      You should have received a copy of the GNU General Public License
//      along with this program; if not, write to the Free Software
//      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
//      MA 02110-1301, USA.

#ifndef UBLAS_HELPER_ONLY_H
#define UBLAS_HELPER_ONLY_H

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <boost/numeric/ublas/io.hpp>

#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/triangular.hpp>

namespace ublas = boost::numeric::ublas;

#define vec_t ublas::vector<double>
#define zero_vec_t ublas::zero_vector<double>
#define unit_vec_t ublas::unit_vector<double>
#define scalar_vec_t ublas::scalar_vector<double>
#define sparse_vec_t ublas::compressed_vector<double>

#define matrix_t ublas::matrix<double>
#define zero_matrix_t ublas::zero_matrix<double>
#define identity_matrix_t ublas::identity_matrix<double>
#define scalar_matrix_t ublas::scalar_matrix<double>
#define sparse_matrix_t ublas::compressed_matrix<double>

// submatrix
#define row(m, i) (ublas::matrix_row<typeof(m)>(m, i))
#define col(m, i) (ublas::matrix_column<typeof(m)>(m, i))
#define subvector(v, start, n) (ublas::vector_range<typeof(v)>(v, ublas::range(start, n)))

/* Use Cholesky decomposition implementation from
 -   copyright            : (C) 2005 by Gunter Winkler, Konstantin Kutzkow
 -   email                : guwi17@gmx.de
*/
/** \brief decompose the symmetric positive definit matrix A into product L L^T.
 *
 * \param MATRIX type of matrix A
 * \param A input: square symmetric positive definite matrix (only the lower triangle is accessed)
 * \param A output: the lower triangle of A is replaced by the cholesky factor
 * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
 */
template < class MATRIX >
size_t cholesky_decompose(MATRIX& A)
{
  using namespace ublas;

  typedef typename MATRIX::value_type T;
  
  const MATRIX& A_c(A);

  const size_t n = A.size1();
  
  for (size_t k=0 ; k < n; k++) {
        
    double qL_kk = A_c(k,k) - inner_prod( project( row(A_c, k), range(0, k) ),
                                          project( row(A_c, k), range(0, k) ) );
    
    if (qL_kk <= 0) {
      return 1 + k;
    } else {
      double L_kk = sqrt( qL_kk );
      
      matrix_column<MATRIX> cLk(A, k);
      project( cLk, range(k+1, n) )
        = ( project( column(A_c, k), range(k+1, n) )
            - prod( project(A_c, range(k+1, n), range(0, k)), 
                    project(row(A_c, k), range(0, k) ) ) ) / L_kk;
      A(k,k) = L_kk;
    }
  }
  return 0;      
}

/** \brief solve system L L^T x = b inplace
 *
 * \param L a triangular matrix
 * \param x input: right hand side b; output: solution x
 */
template < class TRIA, class VEC >
void
cholesky_solve(const TRIA& L, VEC& x, ublas::lower)
{
  using namespace ublas;
//   ::inplace_solve(L, x, lower_tag(), typename TRIA::orientation_category () );
  inplace_solve(L, x, lower_tag() );
  inplace_solve(trans(L), x, upper_tag());
}

// 
struct CommonFunction {

  static double KLdivergence(const vec_t& p, const vec_t& q)
  {
    double sum = 0;
    for (int i = 0; i < sz(p) && i < sz(q); i++) {
      sum += p(i) * ( log(p(i)) - log(q(i)) );
    }
    return sum;
  }
};

#endif
